package es.uoc.mxpdroid.algorithm;

import java.util.ArrayList;

import es.uoc.mxpdroid.entity.GridCell;
import es.uoc.mxpdroid.entity.IntRasterLayer;
import es.uoc.mxpdroid.entity.RasterLayer;
import es.uoc.mxpdroid.util.GeoUTM;
import es.uoc.mxpdroid.util.Logger;


public class AccCostAnisotropicAlgorithm {

   public static final String  COST         = "COST";
   public static final String  COSTDIR      = "COSTDIR";
   public static final String  KFACTOR      = "KFACTOR";
   public static final String  FEATURES     = "FEATURES";
   public static final String  ACCCOST      = "ACCCOST";
   public static final String  CLOSESTPOINT = "CLOSESTPOINT";

   private final static int    m_iOffsetX[] = { 0, 1, 1, 1, 0, -1, -1, -1 };
   private final static int    m_iOffsetY[] = { 1, 1, 0, -1, -1, -1, 0, 1 };
   private final static double m_dDist[]    = { 1, Math.sqrt(2.), 1, Math.sqrt(2.), 1, Math.sqrt(2.), 1, Math.sqrt(2.) };

   private static final int    NO_DATA      = -1;
   private static final int    DATA         = 1;
   private static final double ANGLES[][]   = { { 135, 180, 225 }, { 90, 0, 270 }, { 45, 0, 315 } };

   private int                m_iNX, m_iNY;
   private double             m_dK;
   private RasterLayer        m_Cost;
   private RasterLayer        m_CostDir;
   //private RasterLayer        m_EndPoints;
   private RasterLayer        m_AccCost;
   //private RasterLayer        m_ClosestPoint;
   //private RasterLayer     m_Points, m_Points2;
   private IntRasterLayer     m_Points, m_Points2;
   private ArrayList          m_AdjPoints, m_CentralPoints;
   private double			  m_lat;
   private double			  m_lon;



//   public AccCostAnisotropicAlgorithm(RasterLayer cost, RasterLayer costDir,
//		   RasterLayer end_points, double kfactor) {
	 public AccCostAnisotropicAlgorithm(RasterLayer cost, RasterLayer costDir,
			   double destLat, double destLon, double kfactor) {
	   m_Cost = cost;
	   m_CostDir = costDir;
	   //m_EndPoints = end_points;
	   m_lat = destLat;
	   m_lon = destLon;
	   
	   m_AccCost = new RasterLayer(m_Cost.getLayerType());
	   //m_ClosestPoint = new RasterLayer(m_Cost.getLayerType());
	   m_Points = new IntRasterLayer (m_Cost.getLayerType());
	   m_Points2 = new IntRasterLayer (m_Cost.getLayerType());
	   m_dK = kfactor;
	   
	   m_AdjPoints = new ArrayList();
	   m_CentralPoints = new ArrayList();

	   m_iNX = m_Cost.getNX();
	   m_iNY = m_Cost.getNY();
	   
	   //initialize the layers with empty data according to the matrix dimension
	   m_AccCost.initFromLayer(m_Cost);
	   
	   //m_ClosestPoint.initFromLayer(m_Cost);
	   
	   m_Points.initFromLayer(m_Cost);
	   m_Points2.initFromLayer(m_Cost);
	   
	   processAlgorithm();
   }


   private void processAlgorithm() {

//      int x, y;
//      int iPoint = 1;
//      double dValue;

//      m_ClosestPoint.assign(0);
//
//      for (y = 0; y < m_iNY; y++) {
//         for (x = 0; x < m_iNX; x++) {
//            dValue = m_EndPoints.getCellValueAsDouble(x, y);
//            if ((dValue != 0.0) && !m_EndPoints.isNoDataValue(dValue)) {
//               m_Points.setCellValue(x, y, DATA);
//               m_CentralPoints.add(new GridCell(x, y, iPoint));
//               m_AccCost.setCellValue(x, y, 0.0);
//               m_ClosestPoint.setCellValue(x, y, iPoint);
//               iPoint++;
//            }
//         }
//      }
      
      m_Points.setDataAtLatLon(m_lat, m_lon, DATA);
      double xyUTM[] = (new GeoUTM(GeoUTM.WGS84)).passToUTM(m_lat, m_lon, 31);
      int xyCell[] = m_Points.get_iijj(xyUTM[0], xyUTM[1]);
      m_CentralPoints.add(new GridCell(xyCell[0], xyCell[1], 1));
      m_AccCost.setDataAtLatLon(m_lat, m_lon, 0.0);
      calculateCost();

   }


   private void calculateCost() {

      int i;
      int iPt;
      int iPoint;
      int x, y, x2, y2;
      double dAccCost;
      double dPrevAccCost;
      GridCell cell;

      while ((m_CentralPoints.size() != 0) ) {
         for (iPt = 0; iPt < m_CentralPoints.size(); iPt++) {
            cell = (GridCell) m_CentralPoints.get(iPt);
            x = cell.getX();
            y = cell.getY();
            iPoint = (int) cell.getValue();
            if (m_Points.getCellValueAsInt(x, y) == DATA) {
               m_Points.setCellValue(x, y, IntRasterLayer.NO_DATA_VALUE);
               for (i = 0; i < 8; i++) {
                  x2 = x + m_iOffsetX[i];
                  y2 = y + m_iOffsetY[i];
                  dAccCost = m_AccCost.getCellValueAsDouble(x, y);
                  dPrevAccCost = m_AccCost.getCellValueAsDouble(x2, y2);
                  if ((dPrevAccCost > dAccCost) || m_AccCost.isNoDataValue(dPrevAccCost)) {
                     final double dCostInDir = getCostInDir(x, y, m_iOffsetX[i], m_iOffsetY[i]);
                     if (dCostInDir != RasterLayer.NO_DATA_VALUE) {
                        dAccCost += dCostInDir * m_dDist[i];
                        if (m_AccCost.isNoDataValue(dPrevAccCost) || (dPrevAccCost > dAccCost)) {
                           m_AccCost.setCellValue(x2, y2, dAccCost);
//                           m_ClosestPoint.setCellValue(x2, y2, iPoint);
//                           if (m_Points2.getCellValueAsInt(x2, y2) == RasterLayer.NO_DATA_VALUE) {
                        	   if (m_Points2.getCellValueAsInt(x2, y2) == IntRasterLayer.NO_DATA_VALUE) {	   
                              m_Points2.setCellValue(x2, y2, DATA);
                              m_AdjPoints.add(new GridCell(x2, y2, iPoint));
                           }
                        }
                     }
                  }
               }
            }
         }

         final IntRasterLayer swap = m_Points;
         m_Points = m_Points2;
         m_Points2 = swap;

         m_CentralPoints = m_AdjPoints;
         m_AdjPoints = new ArrayList();

      }
   }


   private double getCostInDir(final int x,
                               final int y,
                               final int iH,
                               final int iV) {

      final double dAngle = ANGLES[iV + 1][iH + 1];

      final int x2 = x + iH;
      final int y2 = y + iV;

      double dCost1 = m_Cost.getCellValueAsDouble(x, y);
      double dCost2 = m_Cost.getCellValueAsDouble(x2, y2);

      final double dCostDir1 = m_CostDir.getCellValueAsDouble(x, y);
      final double dCostDir2 = m_CostDir.getCellValueAsDouble(x2, y2);

      if (m_Cost.isNoDataValue(dCost1) || m_Cost.isNoDataValue(dCost2) || m_CostDir.isNoDataValue(dCostDir1)
          || m_CostDir.isNoDataValue(dCostDir2) || (dCost1 <= 0) || (dCost2 <= 0)) {
         return RasterLayer.NO_DATA_VALUE;
      }
      else {
         double dDifAngle1 = Math.abs(dCostDir1 - dAngle);
         double dDifAngle2 = Math.abs(dCostDir2 - dAngle);

         dDifAngle1 = Math.toRadians(dDifAngle1);
         dDifAngle2 = Math.toRadians(dDifAngle2);

         final double dCos1 = Math.cos(dDifAngle1);
         final double dCos2 = Math.cos(dDifAngle2);

         final int dSgn1 = signum(dCos1);
         final int dSgn2 = signum(dCos2);

         final double dExp1 = dSgn1 * Math.pow(Math.abs(dCos1), m_dK);
         final double dExp2 = dSgn2 * Math.pow(Math.abs(dCos2), m_dK);

         dCost1 = Math.pow(dCost1, dExp1) / 2.;
         dCost2 = Math.pow(dCost2, dExp2) / 2.;

         return dCost1 + dCost2;
      }

   }


   private int signum(final double difAngle) {

      if (difAngle < 0) {
         return -1;
      }
      else {
         return 1;
      }

   }
   
   public RasterLayer getResultLayer(){
       return m_AccCost;
   }

}
